/**
  *@file bsp_can.c
  *@date 2019-08-12
  *@author Albert.D
  *@brief
  */
#include "can.h"
#include "bsp_app.h"
#include "bsp_can.h"
#include "bsp_led.h"
#include "FreeRTOS.h"
#include "queue.h"
#include "string.h"
#include "chassis_protocol.h"
extern QueueHandle_t xQueueCan;
extern QueueHandle_t xQueueCan2;

//uint8_t can1_rx_data[8];
//uint8_t can2_rx_data[8];

//can filter must be initialized before use
void CanFilter_Init(CAN_HandleTypeDef* hcan)
{
  CAN_FilterConfTypeDef canfilter;
  //create memory to save the message, if not will raise error
  static CanTxMsgTypeDef  Tx1Message;
  static CanRxMsgTypeDef  Rx1Message;
  static CanTxMsgTypeDef  Tx2Message;
  static CanRxMsgTypeDef  Rx2Message;
  canfilter.FilterMode = CAN_FILTERMODE_IDMASK;
  canfilter.FilterScale = CAN_FILTERSCALE_32BIT;
  //filtrate any ID you want here
  canfilter.FilterIdHigh = 0x0000;
  canfilter.FilterIdLow = 0x0000;
  canfilter.FilterMaskIdHigh = 0x0000;
  canfilter.FilterMaskIdLow = 0x0000;
  canfilter.FilterFIFOAssignment = CAN_FilterFIFO0;
  canfilter.FilterActivation = ENABLE;
  canfilter.BankNumber = 14;

  //use different filter for can1&can2
  if(hcan == &hcan1)
  {
    canfilter.FilterNumber = 0;
    hcan->pTxMsg = &Tx1Message;
    hcan->pRxMsg = &Rx1Message;
  }

  if(hcan == &hcan2)
  {
    canfilter.FilterNumber = 14;
    hcan->pTxMsg = &Tx2Message;
    hcan->pRxMsg = &Rx2Message;
  }

  HAL_CAN_ConfigFilter(hcan, &canfilter);
}

//it will be auto callback when can receive msg completely
void HAL_CAN_RxCpltCallback(CAN_HandleTypeDef* hcan)
{
  userCanBuf_t usrRecvBuf;;
//  BaseType_t xResult;
  BaseType_t xHigherPriorityTaskWoken = pdFALSE;
  {
    __HAL_CAN_CLEAR_FLAG(hcan,CAN_FLAG_FOV0);
    __HAL_CAN_ENABLE_IT(hcan, CAN_IT_FOV0 | CAN_IT_FMP0);

    if(hcan1.Instance ==  hcan->Instance)
    {
      LED_Red_Toggle();
      usrRecvBuf.port = CAN1_PORT;
    }
    else if(hcan2.Instance ==  hcan->Instance)
    {
      LED_Green_Toggle();
//        SEGGER_RTT_printf(0,"can_id = %x\r\n",hcan->pRxMsg->StdId);
      usrRecvBuf.port = CAN2_PORT;
    }

    usrRecvBuf.ID = hcan->pRxMsg->StdId;
    memcpy(usrRecvBuf.can_data,hcan->pRxMsg->Data,8);
    xQueueSendFromISR(xQueueCan,(void *)&usrRecvBuf,&xHigherPriorityTaskWoken);
    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
  }
}

//CAN send message test
uint8_t CAN_Send_Msg(CAN_HandleTypeDef* hcan, uint8_t *msg, uint32_t id, uint8_t len)
{
  uint8_t index = 0;
  hcan->pTxMsg->StdId = id;
  hcan->pTxMsg->IDE = CAN_ID_STD;
  hcan->pTxMsg->RTR = CAN_RTR_DATA;
  hcan->pTxMsg->DLC = len;

  for(index = 0; index <len; index++)
    hcan->pTxMsg->Data[index] = msg[index];

  if(HAL_CAN_Transmit_IT(hcan) == HAL_OK)
  {
    return 0;
  }

  //HAL_CAN_Transmit(hcan, 10);
  return 1;
}
uint8_t chasis_vel_ctl(userCanBuf_t* can_buff)
{
  //CAN_Send_Msg(&hcan2, chassisBuf.can_data, chassisBuf.ID, 8);
  return CAN_Send_Msg(&hcan2,can_buff->can_data,can_buff->ID,8);
}

uint8_t chasis_repeort_vel(userCanBuf_t* can_buff)
{
  return CAN_Send_Msg(&hcan1,can_buff->can_data,can_buff->ID,8);
}


